function [out,errorMes]=CDKF_dsge(model,setupFilter,yData,x0,P0,Rw)
%   This function performs the Central Difference Kalman filter; a state estimation for nonlinear 
%   systems that is based on second-order polynomial approximations of the 
%   nonlinear mappings. The approximations are derived by using a 
%   multidimensional extension of Stirling's interpolation formula. 
%   The model of the nonlinear system must be specified in the form:
%               x(k+1) = h[x(k)]+v(k+1)
%               y(k)   = g[x(k),z(k)]+w(k)
%   where 'x' is the state vector, y is the control vector
%   Finally, 'v' and 'w'  are (white) noise sources.
%
% Output
%   xhat    - State estimates. Dimension is [states, samples].
%             Notice: in xhat, the first row is for observation 0.
%   Smat    - Matrix with the square root of the covariance matrix for the
%             posterior state estimates. The dimension is [dimx, dimx, samples] 
%   LogL    - The contribution to SumLogL for each observation
%
%
%  The user must write the m-functions 'kalmfilex' and 'kalmfiley' 
%
%  Literature:
%     M. Norgaard, N.K. Poulsen, O. Ravn: "New Developments in State
%     Estimation for Nonlinear Systems", Automatica, (36:11), Nov. 2000,
%     pp. 1627-1638.
%
% >>>>>>>>>>>>>>>>>>>>>>>>>>> INITIALIZATIONS <<<<<<<<<<<<<<<<<<<<<<<<<<
out       = [];
h2        = 3;                        % Squared divided-difference step size
h         = sqrt(h2);                 % Divided difference step-size
scal1     = 0.5/h;                    % A convenient scaling factor
scal2     = sqrt((h2-1)/(4*h2*h2));   % Another scaling factor
samples   = size(yData,1);            % The samples length
dimy      = size(yData,2);            % # of observations at each time point
dimx      = size(P0,1);               % # of states
Rv        = model.eta*model.eta';     % The covariance matrix for the structural shocks
dimw      = size(Rw,1);               % # of measurement noise sources
[v,d]     = eig(P0);                  % Square root of initial state covariance
Sx        = triag(real(v*sqrt(d)));
if setupFilter.pruningOn == 1
    if model.appOrder == 1
        RvAll     = Rv;        
    elseif model.appOrder== 2
        RvAll     = [Rv                          zeros(model.nx,1*model.nx1);
                     zeros(1*model.nx1,model.nx) zeros(1*model.nx1,1*model.nx1)];
    elseif model.appOrder == 3
        RvAll     = [Rv                          zeros(model.nx,2*model.nx1);
                     zeros(2*model.nx1,model.nx) zeros(2*model.nx1,2*model.nx1)];
    end
    [v,d]     = eig(RvAll);                  % Square root of process noise covariance
    Sv        = real(v*sqrt(d));
    dimv      = size(RvAll,1);            % # of process noise sources
else
    [v,d]     = eig(Rv);                  % Square root of process noise covariance
    Sv        = real(v*sqrt(d));
    dimv      = size(Rv,1);               % # of process noise sources
end

%[v,d]     = eig(Rw);                  % Square root of measurement noise cov.
%Sw        = real(v*sqrt(d));    
Sw        = diag(sqrt(diag(Rw)));
diagSw    = diag(Sw);         

% Setting the dimensions
SxxSxv    = zeros(dimx,2*dimx+dimv);            % Allocate compund matrix consisting of Sxx and Syv 
SyxSyw    = zeros(dimy,2*dimx+dimw);            % Allocate compund matrix consisting of Syx and Syw
xhat      = zeros(dimx,samples);                % Matrix for storing posterior state estimates
xbar      = zeros(dimx,samples);                % Matrix for storing a priori state estimates
Smat      = zeros(dimx,dimx,samples);           % Matrix for storing posterior cov. matrices
LogL      = zeros(samples,1);                   % Vector for storing the contributions to the log likelihood fct
invPyy    = NaN(dimy,dimy,samples);             % Array for storing the inv of Pyy
Pyy       = NaN(dimy,dimy,samples);             % Array for storing Pyy
inov      = NaN(dimy,samples);                  % Innovations
ybar      = NaN(dimy,samples);                  % Vector for storing the predictions of y
Kgain_mat = NaN(dimx,dimy);
% Linear process noise model in state equation: x(k+1) = h(x(k))+v(k+1)
SxxSxv(:,dimx+1:dimx+dimv) = Sv;

% >>>>>>>>>>>>>>>>>>>>>>>>>>>> FILTERING <<<<<<<<<<<<<<<<<<<<<<<<<<<
errorMes = 0;
for k=1:samples
    if k == 1
        fxbar = stateTransition(x0,model);
        xbar = ((h2-dimx)/h2)*fxbar;
        kx2 = dimx+dimv;
        for kx=1:dimx
            sxp  = stateTransition(x0+h*Sx(:,kx),model);
            sxm  = stateTransition(x0-h*Sx(:,kx),model);
            SxxSxv(:,kx)     = scal1*(sxp-sxm);
            SxxSxv(:,kx2+kx) = scal2*(sxp+sxm-2*fxbar);
            xbar(:,k)        = xbar(:,k) + (sxp+sxm)/(2*h2);
        end
    else
        % Update Sv to account for potential heteroskedasticity if needed
        if any(model.heteroShocks~= 0)
            select = model.nx1+1:model.nx;
            tmp    = (2./(exp(-model.heteroShocks(:).*xhat(select,k-1))+1)).^2.*diag(Rv(select,select));
            if any(isnan(tmp)) || any(isinf(tmp))
                disp(['Unable to update Rv, at t = ',num2str(k)]);
                errorMes = 1;
                return;
            end
            RvAll(select,select) = diag(tmp);
            [v,d]     = eig(RvAll);                  % Square root of process noise covariance
            Sv        = real(v*sqrt(d));
        end
        % Linear process noise model in state equation: x(k+1) = h(x(k))+v(k+1)
        SxxSxv(:,dimx+1:dimx+dimv) = Sv;    
        fxbar = stateTransition(xhat(:,k-1),model);
        xbar(:,k) = ((h2-dimx)/h2)*fxbar;
        kx2 = dimx+dimv;
        for kx=1:dimx
            sxp = stateTransition(xhat(:,k-1)+h*Sx(:,kx),model);
            sxm = stateTransition(xhat(:,k-1)-h*Sx(:,kx),model);
            SxxSxv(:,kx)     = scal1*(sxp-sxm);
            SxxSxv(:,kx2+kx) = scal2*(sxp+sxm-2*fxbar);
            xbar(:,k)        = xbar(:,k) + (sxp+sxm)/(2*h2);
        end
    end
    
    % Cholesky factor of a'priori estimation error covariance
    if setupFilter.MEXon == 1
        Sxbar = triag_Mex(SxxSxv,int32(size(SxxSxv,1)),int32(size(SxxSxv,2)));
    else
        Sxbar = triag(SxxSxv);
    end
    
    % --- Measurement update (a posteriori update) ---
    % Accounting for missing values
    yAll       = yData(k,:)';
    select     = ~isnan(yAll);
    numObs     = sum(select,1);
    y          = yAll(select,1);

    % Linear observation noise model: y(k) = g(x(k)) + w(k)
    dimw             = numObs;
    %SyxSyw(1:numObs,dimx+1:dimx+dimw) = diag(diagSw(select));
    if setupFilter.timeIndex(k,:) >= datenum(1979,10,1) && setupFilter.timeIndex(k,:) <= datenum(1982,10,1)
        % The monetary experiment 
        diagSwUsed = diagSw;
        diagSwUsed(5) = 0.0100;   %100 bp of m-error for short rate
    else
        diagSwUsed = diagSw;
    end
    if isfield(setupFilter,'zlbON')
        if setupFilter.zlbON == 1 && y(5,1) < 0.005
            % Short rate at the ZLB, i.e. r<50 bp
            diagSwUsed = diagSw;
            diagSwUsed(5) = 0.0005;   %5 bp of m-error for short rate
        end
    end
    SyxSyw(1:numObs,dimx+1:dimx+dimw) = diag(diagSwUsed(select));
    y0All = modelFit(xbar(:,k),model);
    ybar(1:numObs,k) = ((h2-dimx)/h2)*y0All(select,1);
    kx2 = dimx+dimw;
    for kx=1:dimx
        sypAll = modelFit(xbar(:,k)+h*Sxbar(:,kx),model);
        symAll = modelFit(xbar(:,k)-h*Sxbar(:,kx),model);
        SyxSyw(1:numObs,kx)     = scal1*(sypAll(select,1)-symAll(select,1));
        SyxSyw(1:numObs,kx2+kx) = scal2*(sypAll(select,1)+symAll(select,1)-2*y0All(select,1));
        ybar(1:numObs,k)        = ybar(1:numObs,k)+(sypAll(select,1)+symAll(select,1))/(2*h2);
    end
        
    % Cholesky factor of a'posteriori output estimation error covariance
    if setupFilter.MEXon == 1
        Sy = triag_Mex(SyxSyw(1:numObs,1:kx2+dimx),int32(numObs),int32(kx2+dimx));
    else
        Sy = triag(SyxSyw(1:numObs,1:kx2+dimx));
    end
    Pyy(1:numObs,1:numObs,k) = Sy*Sy';
    invPyy(1:numObs,1:numObs,k) = Pyy(1:numObs,1:numObs,k)\eye(numObs);
    Kgain  = (Sxbar*SyxSyw(1:numObs,1:dimx)')*invPyy(1:numObs,1:numObs,k);
    
    % Contribution to the quasi log-likelihood function
    detPyy = det(Pyy(1:numObs,1:numObs,k));
    if detPyy <= 1D-300 || abs(imag(detPyy))
        disp(['Invertibility problems in CKDF, at t = ',num2str(k),' detPyy = ',num2str(detPyy)]);
        errorMes = 1;
        return;
    end
    inov(1:numObs,k) = y - ybar(1:numObs,k);
    LogL(k,1) = -0.5*numObs*log(2*pi)-0.5*log(detPyy)-...
        0.5*inov(1:numObs,k)'*invPyy(1:numObs,1:numObs,k)*inov(1:numObs,k);
    
    % Update of the state estimate - i.e. posterior state estimate
    xhat(:,k) = xbar(:,k) + Kgain*inov(1:numObs,k);
    
    % Cholesky factor of a'posteriori estimation error covariance
    if setupFilter.MEXon == 1
        tmp   = [Sxbar-Kgain*SyxSyw(1:numObs,1:dimx) Kgain*SyxSyw(1:numObs,dimx+1:kx2+dimx)];
        Sx    = triag_Mex(tmp,int32(size(tmp,1)),int32(size(tmp,2)));
    else
        Sx    = triag([Sxbar-Kgain*SyxSyw(1:numObs,1:dimx) Kgain*SyxSyw(1:numObs,dimx+1:kx2+dimx)]);
    end
    
    % Store the covariances for the state estimate
    Smat(:,:,k) = Sx;
    Kgain_mat(1:dimx,1:numObs,k) = Kgain;
end

% Saving output
out.LogL   = LogL;
out.xhat   = xhat;
out.Smat   = Smat;
out.xbar   = xbar;
out.ybar   = ybar;
out.invPyy = invPyy;
out.Pyy    = Pyy;
out.inov   = inov;
out.Kgain_mat = Kgain_mat;
end